#include "ros/ros.h"
#include "std_msgs/String.h"

#include <geometry_msgs/PointStamped.h>
#include <sstream>
#include <iostream>
#include <vector>
#include <string>
#include <fstream>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>

using namespace std;
 vector<double> v;
 vector<vector<double>> v_path(8);
ros::Publisher evo_odom_pub;
void traj_odom_pub(const nav_msgs::Path::ConstPtr& msg){
   nav_msgs::Odometry odom_msgs;
  /* 
   odom_msgs.header.stamp = msg->header.stamp;
   odom_msgs.header.frame_id="world";
   odom_msgs.child_frame_id="world";
   odom_msgs.pose.pose.position.x=msg->poses.pose.position.x;
   odom_msgs.pose.pose.position.y=msg->poses.pose.position.y;
   odom_msgs.pose.pose.position.z=msg->poses.pose.position.z;
   evo_odom_pub.publish(odom_msgs);
*/
}
void LoadImages(const string &trajectory, vector<double> &position_x)
{
    ifstream fdata;

    // 数据流读取文件
    fdata.open(trajectory.c_str());
    position_x.reserve(10000);
    double d;
    while(fdata >> d)position_x.push_back(d);
    fdata.close();
    int cnum=position_x.size()/8;
    cout<<"cnum"<<cnum;
    v_path.resize(1000);

    for(int i=0;i<v_path.size();i++){
      for(int j=0;j<8;j++){
      v_path[i].push_back(position_x[8*i+j]);
      }
       
    }
}
 
 
int main(int argc, char **argv){


    vector<double> vposition;
 
    // LoadImages(string(argv[1]),vposition);
    LoadImages("/home/tarmy/transh/papera/catkin_simple/src/beginner_tutorials/data/basketball_filter.txt",vposition);
 
    ros::init(argc,argv,"publish_02"); //初始化节点
    ros::NodeHandle n; 
    ros::Publisher chatter_pub = n.advertise<geometry_msgs::PointStamped>("clicked_point",1); //定义发送端
    ros::Publisher path_pub = n.advertise<nav_msgs::Path>("trajectory",1, true);
    //ros::Subscriber evo_msg_sub = n.subscribe("/Tarmy/personTrajectory", 100, traj_odom_pub);
   // evo_odom_pub=n.advertise<nav_msgs::Odometry>("odom_traj",10, true);    
ros::Rate loop_rate(100); //频率
    //vector<string>::iterator i = vposition.begin();
    //auto i = vposition.begin();

    nav_msgs::Path path;
   // nav_msgs::Odometry odom_msgs;
      for(int i=0;i<v_path.size();i++){
            auto current_time = ros::Time::now();
	    path;
	    path.header.stamp=current_time;
	    path.header.frame_id="world";
	    geometry_msgs::PoseStamped this_pose_stamped;
	    this_pose_stamped.pose.position.x = v_path[i][1];
	    this_pose_stamped.pose.position.y = v_path[i][2];
	    this_pose_stamped.pose.position.z = v_path[i][3];
	    this_pose_stamped.header.stamp=current_time;
	    this_pose_stamped.header.frame_id="world";
	    path.poses.push_back(this_pose_stamped);
	    path_pub.publish(path);
            //publish odom

      }    
    while (ros::ok()){
        int i=0;
        vector<double>::iterator it;
        for(it = vposition.begin();it != vposition.end();it++)
     {
         cout << "vposition[" << i << "]="<< setprecision(16) << *it << endl;
             geometry_msgs::PointStamped msg;
    msg.header.frame_id = "world";
    // int num = vposition.size();



         i++;
     }


         
        ROS_INFO("0-------------");
        ROS_INFO("i = %d \n",i); 
        ros::spinOnce();
        loop_rate.sleep();
	}

    return 0;
}

